﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace Bluefly
{

    public interface IDatasource
    {
        double Value { get; }
    
    
    }

    public class KalmanFilteredAltitude : IDatasource
    {
        public const  double STANDARD_SEA_LEVEL_PRESSURE = 101325;  //in pascals
        private double _seaLevelPressure;

        private double rawPressure;
        private double rawAltitude;
        private double dampedAltitude;
        private double kalmanAltitude;

        private bool dampedAltStarted = false;
        private String name;
        private KalmanFilter kalmanFilter;

        private KalmanFilteredVario kalmanVario;
        private KalmanFilteredVario dampedVario;
        
        private double positionNoise = 0.2;
        private double accelerationNoise = 1.0;

        private double altDamp = 0.05;

        const double AltPowFactor = 5.255;
        const double ReciprocalAltPowerFactor = 1 / AltPowFactor;
        const double AltConstant = 44330.0;


        public KalmanFilteredAltitude(double seaLevelPressure, string name)
        {
            this._seaLevelPressure = seaLevelPressure == 0 ? STANDARD_SEA_LEVEL_PRESSURE: seaLevelPressure;
            
            this.name = name;
            kalmanFilter = new KalmanFilter(accelerationNoise);
            kalmanVario = new KalmanFilteredVario(this, 1.0, KalmanFilteredVario.VarioType.KALMAN_VARIO);
            dampedVario = new KalmanFilteredVario(this, 0.05, KalmanFilteredVario.VarioType.DAMPED_VARIO);
        }


        public double addPressure(double pressure, double time)
        {

            rawPressure = pressure;
            rawAltitude = AltConstant * (1 - Math.Pow((pressure / _seaLevelPressure), ReciprocalAltPowerFactor));

            kalmanFilter.update(rawAltitude, positionNoise, 0.02);
            kalmanAltitude = kalmanFilter.getXAbs();


            if (dampedAltStarted)
            {
                dampedAltitude = dampedAltitude + altDamp * (rawAltitude - dampedAltitude);
            }
            else
            {
                dampedAltitude = rawAltitude;
                dampedAltStarted = true;
            }

            
            kalmanVario.addData(time);
            dampedVario.addData(time);

            return rawAltitude;
        }

        /**
         * Sets the altitude to a specific level in m and updates the sea level pressure. Returns the new
         * sea level pressure value.
         */
        public double setAltitude(double alt)
        {

            rawPressure = rawPressure == 0 ? STANDARD_SEA_LEVEL_PRESSURE : rawPressure;

            double qnh = rawPressure / Math.Pow(1.0 - (alt / AltConstant), AltPowFactor);
            rawAltitude = alt;
            dampedAltitude = rawAltitude;
            dampedAltStarted = true;
            _seaLevelPressure = qnh;

            return _seaLevelPressure;

        }

        public double getSeaLevelPressure()
        {
            return _seaLevelPressure;
        }

        public double setSeaLevelPressure(double seaLevelPressure)
        {
            this._seaLevelPressure = seaLevelPressure;

            dampedAltStarted = false;

            return AltConstant * (1 - Math.Pow((rawPressure / _seaLevelPressure), ReciprocalAltPowerFactor));
        }


        public double getRawPressure()
        {
            return rawPressure;
        }

        public double getRawAltitude()
        {
            return rawAltitude;
        }

        public double DampedAltitude
        {
            get{ return dampedAltitude;}
        }

        public bool IsDampedAltStarted
        {
            get { return dampedAltStarted; }
        }

        public String getName()
        {
            return name;
        }

        public KalmanFilteredVario KalmanVario
        {
            get { return kalmanVario; }
        }

        public KalmanFilteredVario DampedVario
        {
            get { return dampedVario; }
        }

        public double getVarioValue()
        {
            return kalmanFilter.getXVel();
        }

        public double getPositionNoise()
        {
            return positionNoise;
        }

        public void setPositionNoise(double positionNoise)
        {
            this.positionNoise = positionNoise;
        }

        public String toString()
        {
            return "Alt:" + name + ":" + rawAltitude;
        }

        public double Value
        {
            get { return dampedAltitude;}
        }

        public double getAltDamp()
        {
            return altDamp;
        }

        public void setAltDamp(double altDamp)
        {
            this.altDamp = altDamp;
        }



    }
}
